#ifdef ARDUINO
#include <Arduino.h>
#include <core_debug.h>
#endif
#include <stdio.h>
#include "plc_mb_addr.h"
#include "mb_slave_and_master.h"
#include "mb_layer1.h"
#include "mb_slave_private.h"
#include "mb_master_private.h"
#include "mb_slave.h"
#include "mb_master.h"
#include "mb_slave_and_master.h"
#include "mb_util.h"

#define MAX_MODBUS_ERROR_CODE 11
static const char *modbus_error_messages[MAX_MODBUS_ERROR_CODE + 1] = {
    /* 0 */ "", /* un-used -> no error! */
    /* 1 */ "illegal/unsuported function",
    /* 2 */ "illegal data address",
    /* 3 */ "illegal data value",
    /* 4 */ "slave device failure",
    /* 5 */ "acknowledge -> slave intends to reply later",
    /* 6 */ "slave device busy",
    /* 7 */ "negative acknowledge",
    /* 8 */ "memory parity error",
    /* 9 */ "", /* undefined by Modbus */
    /* 10*/ "gateway path unavalilable",
    /* 11*/ "gateway target device failed to respond"};

/* Execute a modbus client transaction/request */
static int __execute_mb_request(client_request_t *request, int ttyfd)
{
    switch (request->mb_function)
    {

    case 1: /* read coils */
        return read_output_bits(request->slave_id,
                                request->address,
                                request->count,
                                request->coms_buffer,
                                (int)request->count,
                                ttyfd,
                                request->retries,
                                &(request->error_code),
                                &(request->resp_timeout),
                                &(request->coms_buf_mutex));

    case 2: /* read discrete inputs */
        return read_input_bits(request->slave_id,
                               request->address,
                               request->count,
                               request->coms_buffer,
                               (int)request->count,
                               ttyfd,
                               request->retries,
                               &(request->error_code),
                               &(request->resp_timeout),
                               &(request->coms_buf_mutex));

    case 3: /* read holding registers */
        return read_output_words(request->slave_id,
                                 request->address,
                                 request->count,
                                 request->coms_buffer,
                                 (int)request->count,
                                 ttyfd,
                                 request->retries,
                                 &(request->error_code),
                                 &(request->resp_timeout),
                                 &(request->coms_buf_mutex));

    case 4: /* read input registers */
        return read_input_words(request->slave_id,
                                request->address,
                                request->count,
                                request->coms_buffer,
                                (int)request->count,
                                ttyfd,
                                request->retries,
                                &(request->error_code),
                                &(request->resp_timeout),
                                &(request->coms_buf_mutex));

    case 5: /* write single coil */
        return write_output_bit(request->slave_id,
                                request->address,
                                request->coms_buffer[0],
                                ttyfd,
                                request->retries,
                                &(request->error_code),
                                &(request->resp_timeout),
                                &(request->coms_buf_mutex));

    case 6: /* write single register */
        return write_output_word(request->slave_id,
                                 request->address,
                                 request->coms_buffer[0],
                                 ttyfd,
                                 request->retries,
                                 &(request->error_code),
                                 &(request->resp_timeout),
                                 &(request->coms_buf_mutex));

    case 7:
        break; /* function not yet supported */
    case 8:
        break; /* function not yet supported */
    case 9:
        break; /* function not yet supported */
    case 10:
        break; /* function not yet supported */
    case 11:
        break; /* function not yet supported */
    case 12:
        break; /* function not yet supported */
    case 13:
        break; /* function not yet supported */
    case 14:
        break; /* function not yet supported */

    case 15: /* write multiple coils */
        return write_output_bits(request->slave_id,
                                 request->address,
                                 request->count,
                                 request->coms_buffer,
                                 ttyfd,
                                 request->retries,
                                 &(request->error_code),
                                 &(request->resp_timeout),
                                 &(request->coms_buf_mutex));

    case 16: /* write multiple registers */
        return write_output_words(request->slave_id,
                                  request->address,
                                  request->count,
                                  request->coms_buffer,
                                  ttyfd,
                                  request->retries,
                                  &(request->error_code),
                                  &(request->resp_timeout),
                                  &(request->coms_buf_mutex));

    default:
        break; /* should never occur, if file generation is correct */
    }

    core_debug("Modbus plugin: Modbus function %d not supported\n", request->mb_function); /* should never occur, if file generation is correct */
    return -1;
}

/* pack bits from unpacked_data to packed_data */
static inline int __pack_bits(uint16_t *unpacked_data, uint16_t start_addr, uint16_t bit_count, uint8_t *packed_data)
{
    uint8_t bit;
    uint16_t byte, coils_processed;

    if ((0 == bit_count) || (65535 - start_addr < bit_count - 1))
        return -ERR_ILLEGAL_DATA_ADDRESS; /* ERR_ILLEGAL_DATA_ADDRESS defined in mb_util.h */

    for (byte = 0, coils_processed = 0; coils_processed < bit_count; byte++)
    {
        packed_data[byte] = 0;
        for (bit = 0x01; (bit & 0xFF) && (coils_processed < bit_count); bit <<= 1, coils_processed++)
        {
            if (unpacked_data[start_addr + coils_processed])
                packed_data[byte] |= bit; /*   set bit */
            else
                packed_data[byte] &= ~bit; /* reset bit */
        }
    }
    return 0;
}

/* unpack bits from packed_data to unpacked_data */
static inline int __unpack_bits(uint16_t *unpacked_data, uint16_t start_addr, uint16_t bit_count, uint8_t *packed_data)
{
    uint8_t temp, bit;
    uint16_t byte, coils_processed;

    if ((0 == bit_count) || (65535 - start_addr < bit_count - 1))
        return -ERR_ILLEGAL_DATA_ADDRESS; /* ERR_ILLEGAL_DATA_ADDRESS defined in mb_util.h */

    for (byte = 0, coils_processed = 0; coils_processed < bit_count; byte++)
    {
        temp = packed_data[byte];
        for (bit = 0x01; (bit & 0xff) && (coils_processed < bit_count); bit <<= 1, coils_processed++)
        {
            unpacked_data[start_addr + coils_processed] = (temp & bit) ? 1 : 0;
        }
    }
    return 0;
}

static int __read_inbits(memarea *mem_map, uint16_t start_addr, uint16_t bit_count, uint8_t *data_bytes)
{
    return __pack_bits(mem_map->buf, start_addr, bit_count, data_bytes);
}
static int __read_outbits(memarea *mem_map, uint16_t start_addr, uint16_t bit_count, uint8_t *data_bytes)
{
    return __pack_bits(mem_map->buf, start_addr, bit_count, data_bytes);
}
static int __write_outbits(memarea *mem_map, uint16_t start_addr, uint16_t bit_count, uint8_t *data_bytes)
{
    return __unpack_bits(mem_map->buf, start_addr, bit_count, data_bytes);
}

static int __read_inwords(memarea *mem_map, uint16_t start_addr, uint16_t word_count, uint16_t *data_words)
{

    if ((start_addr < mem_map->start_address) || ((start_addr + word_count) > (mem_map->start_address + mem_map->length)))
        return -ERR_ILLEGAL_DATA_ADDRESS; /* ERR_ILLEGAL_DATA_ADDRESS defined in mb_util.h */

    /* use memcpy() because loop with pointers (uint16_t *) caused alignment problems */
    memcpy(/* dest */ (void *)data_words,
           /* src  */ (void *)&(mem_map->buf[start_addr - mem_map->start_address]),
           /* size */ word_count * 2);
    return 0;
}

static int __read_outwords(memarea *mem_map, uint16_t start_addr, uint16_t word_count, uint16_t *data_words)
{

    if ((start_addr < mem_map->start_address) || ((start_addr + word_count) > (mem_map->start_address + mem_map->length)))
        return -ERR_ILLEGAL_DATA_ADDRESS; /* ERR_ILLEGAL_DATA_ADDRESS defined in mb_util.h */

    /* use memcpy() because loop with pointers (uint16_t *) caused alignment problems */
    memcpy(/* dest */ (void *)data_words,
           /* src  */ (void *)&(mem_map->buf[start_addr - mem_map->start_address]),
           /* size */ word_count * 2);
    return 0;
}

static int __write_outwords(memarea *mem_map, uint16_t start_addr, uint16_t word_count, uint16_t *data_words)
{

    if ((start_addr < mem_map->start_address) || ((start_addr + word_count) > (mem_map->start_address + mem_map->length)))
        return -ERR_ILLEGAL_DATA_ADDRESS; /* ERR_ILLEGAL_DATA_ADDRESS defined in mb_util.h */

    /* WARNING: The data returned in the data_words[] array is not guaranteed to be 16 bit aligned.
   *           It is not therefore safe to cast it to an uint16_t data type.
   *           The following code cannot be used. memcpy() is used instead.
   */
    /*
  for (count = 0; count < word_count ; count++)
    ((server_mem_t *)mem_map)->rw_words[count + start_addr] = data_words[count];
  */
    memcpy(/* dest */ (void *)&(mem_map->buf[start_addr - mem_map->start_address]),
           /* src  */ (void *)data_words,
           /* size */ word_count * 2);
    return 0;
}

#ifndef ARDUINO
#include <pthread.h>
#endif

static void __mb_server_thread(const void *_server_node)
{
    server_node_t *server_node = (server_node_t *)_server_node;
    mb_slave_callback_robits_t callbacks_robits = {
        &__read_inbits,
        (server_node->mem_area_robits)};
    mb_slave_callback_rwbits_t callbacks_rwbits = {
        &__read_outbits,
        &__write_outbits,
        (server_node->mem_area_rwbits)};
    mb_slave_callback_rowords_t callbacks_rowords = {
        &__read_inwords,
        (server_node->mem_area_rowords)};
    mb_slave_callback_words_t callbacks_words = {
        &__read_outwords,
        &__write_outwords,
        (server_node->mem_area_words)};
    callbacks_robits.arg = server_node->mem_area_robits;
    callbacks_rwbits.arg = server_node->mem_area_rwbits;
    callbacks_rowords.arg = server_node->mem_area_rowords;
    callbacks_words.arg = server_node->mem_area_words;
// Enable thread cancelation. Enabled is default, but set it anyway to be safe.
#ifndef ARDUINO
    pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
#endif
    // mb_slave_run() should never return!
    mb_slave_run(server_node->mb_nd /* nd */, &callbacks_robits, &callbacks_rwbits, &callbacks_rowords, &callbacks_words, &server_node->slave_id, &server_node->init_state);
#ifdef ARDUINO
    // vTaskDelete(NULL);
#endif
    //	core_debug( "Modbus plugin: Modbus server for node %s died unexpectedly!\n", server_node->location); /* should never occur */
    //	return NULL;
}

#define timespec_add(ts, sec, nsec)   \
    {                                 \
        ts.tv_sec += sec;             \
        ts.tv_nsec += nsec;           \
        if (ts.tv_nsec >= 1000000000) \
        {                             \
            ts.tv_sec++;              \
            ts.tv_nsec -= 1000000000; \
        }                             \
    }

static void __mb_client_thread(const void *arg)
{
    client_node_t *client_node = (client_node_t *)arg;
    int delay_msec = client_node->comm_period; /* comm_period is in ms */
    if (delay_msec < 20)
        delay_msec = 20;

// Enable thread cancelation. Enabled is default, but set it anyway to be safe.
#ifndef ARDUINO
    pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
#endif

    // get the current time
    //#ifdef ARDUINO
    ////	PLC_GetTime(&next_cycle);
    //	unsigned int t = millis();
    //	next_cycle.tv_sec = 0;
    //	next_cycle.tv_nsec = t * 1000;
    //#else
    //	clock_gettime(CLOCK_MONOTONIC, &next_cycle);
    //#endif

    // loop the communication with the client
    while (1)
    {
        /*
		struct timespec cur_time;
		clock_gettime(CLOCK_MONOTONIC, &cur_time);
		core_debug( "Modbus client thread - new cycle (%ld:%ld)!\n", cur_time.tv_sec, cur_time.tv_nsec);
		*/
        int req;
        for (req = 0; req < client_node->req_nrs; req++)
        {
            osDelay(delay_msec);
            /*just do the requests belonging to the client */
            if (!client_node->req[req].enable)
                continue;
            // if (client_node->req[req].client_node_id != client_node->mb_nd)
            //     continue;
            if (client_node->req[req].tick > millis())
                continue;
            client_node->req[req].tick = millis() + client_node->req[req].period;
            client_node->req[req].node_address.addr.rtu.device = client_node->node_address.addr.rtu.device;
            client_node->mb_nd = mb_master_connect(client_node->req[req].node_address);
            osDelay(10);

            int res_tmp = __execute_mb_request(&client_node->req[req], client_node->mb_nd);
            switch (res_tmp)
            {
            case PORT_FAILURE:
            {
                if (res_tmp != client_node->prev_error)
                    core_debug("Modbus plugin: Error connecting Modbus client %s to remote server.\n", client_node->location);
                client_node->prev_error = res_tmp;
                client_node->req[req].errcount++;
                break;
            }
            case INVALID_FRAME:
            {
                if ((res_tmp != client_node->req[req].prev_error) && (0 == client_node->prev_error))
                    core_debug("Modbus plugin: Modbus client request configured at location %s was unsuccesful. Server/slave returned an invalid/corrupted frame.\n", client_node->req[req].location);
                client_node->req[req].prev_error = res_tmp;
                client_node->req[req].errcount++;
                break;
            }
            case TIMEOUT:
            {
                if ((res_tmp != client_node->req[req].prev_error) && (0 == client_node->prev_error))
                    core_debug("Modbus plugin: Modbus client request configured at location %s timed out waiting for reply from server.\n", client_node->req[req].location);
                client_node->req[req].prev_error = res_tmp;
                client_node->req[req].errcount++;
                break;
            }
            case MODBUS_ERROR:
            {
                if (client_node->req[req].prev_error != client_node->req[req].error_code)
                {
                    core_debug("Modbus plugin: Modbus client request configured at location %s was unsuccesful. Server/slave returned error code 0x%2x", client_node->req[req].location, client_node->req[req].error_code);
                    if (client_node->req[req].error_code <= MAX_MODBUS_ERROR_CODE)
                    {
                        core_debug("(%s)", modbus_error_messages[client_node->req[req].error_code]);
                        core_debug(".\n");
                    }
                }
                client_node->req[req].prev_error = client_node->req[req].error_code;
                client_node->req[req].errcount++;
                break;
            }
            default:
            {
                if ((res_tmp >= 0) && (client_node->prev_error != 0))
                {
                    core_debug("Modbus plugin: Modbus client %s has reconnected to server/slave.\n", client_node->location);
                }
                if ((res_tmp >= 0) && (client_node->req[req].prev_error != 0))
                {
                    core_debug("Modbus plugin: Modbus client request configured at location %s has succesfully resumed comunication.\n", client_node->req[req].location);
                }
				if (res_tmp >= 0)
				{
                client_node->prev_error = 0;
                client_node->req[req].prev_error = 0;
                client_node->req[req].okcount++;
				}
				else
				{
                client_node->req[req].prev_error = client_node->req[req].error_code;
                client_node->req[req].errcount++;
 
				}
                break;
            }
            }
            mb_master_close(client_node->mb_nd);
        }
        // Determine absolute time instant for starting the next cycle
        //		struct timespec prev_cycle, now;
        //		prev_cycle = next_cycle;
        //		timespec_add(next_cycle, period_sec, period_nsec);
        /* NOTE A:
		 * When we have difficulty communicating with remote client and/or server, then the communications get delayed and we will
		 * fall behind in the period. This means that when communication is re-established we may end up running this loop continuously
		 * for some time until we catch up.
		 * This is undesirable, so we detect it by making sure the next_cycle will start in the future.
		 * When this happens we will switch from a purely periodic task _activation_ sequence, to a fixed task suspension interval.
		 * 
		 * NOTE B:
		 * It probably does not make sense to check for overflow of timer - so we don't do it for now!
		 * Even in 32 bit systems this will take at least 68 years since the computer booted
		 * (remember, we are using CLOCK_MONOTONIC, which should start counting from 0
		 * every time the system boots). On 64 bit systems, it will take over 
		 * 10^11 years to overflow.
		 */
#ifdef ARDUINO
//		PLC_GetTime(&now);
//		unsigned int t = millis();
//		now.tv_sec = 0;
//		now.tv_nsec = t * 1000;
#else
//		clock_gettime(CLOCK_MONOTONIC, &now);
#endif
        //		if (((now.tv_sec > next_cycle.tv_sec) || ((now.tv_sec == next_cycle.tv_sec) && (now.tv_nsec > next_cycle.tv_nsec)))
        //			/* We are falling behind. See NOTE A above */
        //			|| (next_cycle.tv_sec < prev_cycle.tv_sec)
        //			/* Timer overflow. See NOTE B above */
        //		)
        //		{
        //			next_cycle = now;
        //			timespec_add(next_cycle, period_sec, period_nsec);
        //		}

#ifndef ARDUINO
        //		clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_cycle, NULL);
    }
#else
        //		osDelay(next_cycle.tv_sec * 1000 + next_cycle.tv_nsec / 1000);
    }
    // vTaskDelete(NULL);
#endif

    // humour the compiler.
    //	return NULL;
}

int _mb_master_connect(client_node_t *node)
{
    // node->mb_nd = mb_master_connect(node->node_address);
    osThreadDef(mb_client, __mb_client_thread, osPriorityNormal, 0, 256); //128溢出,
    node->thread_id = osThreadCreate(osThread(mb_client), node);
}
int _mb_master_close(client_node_t *node)
{
    osThreadSuspend(node->thread_id);
    vTaskDelay(100);
    int res = mb_master_close(node->mb_nd);
    vTaskDelay(100);
    osThreadTerminate(node->thread_id);
    vTaskDelay(100);
    return res;
}
mb_master_abi_t mb_master_abi = {
    _mb_master_connect, _mb_master_close};

mb_abi_t mb_abi = {
    mb_slave_and_master_init,
    mb_slave_and_master_done};
int request_init(client_request_t *client)
{
    client->coms_buf_mutex = xSemaphoreCreateMutex();
    return 0;
};
int mb_request_lock(client_request_t *client) { return xSemaphoreTake(client->coms_buf_mutex, -1) == pdTRUE; };
int mb_request_unlock(client_request_t *client) { return xSemaphoreGive(client->coms_buf_mutex); };
int mb_request_close(client_request_t *request)
{
    vSemaphoreDelete(request->coms_buf_mutex);
    return 0;
};

mb_request_abi_t mb_request_abi = {
    request_init,
    mb_request_lock, mb_request_unlock, mb_request_close};

int mb_slave_init(server_node_t *slave)
{
    slave->mb_nd = mb_slave_new(slave->node_address);
    osThreadDef(mb_server, __mb_server_thread, osPriorityNormal, 0, 512); //256不够,384怀疑乱码,使用512测试
    slave->thread_id = osThreadCreate(osThread(mb_server), slave);
    return slave->mb_nd;
};
int mb_slave_close(server_node_t *server)
{
    osThreadSuspend(server->thread_id);
    vTaskDelay(100);
    int res = mb_slave_close(server->mb_nd);
    vTaskDelay(100);
    osThreadTerminate(server->thread_id);
    vTaskDelay(100);
    return res;
};

mb_slave_abi_t mb_slave_abi = {
    mb_slave_init, mb_slave_close};
